    #include "mainwindow.h"
#include <QTimer>
#pragma execution_character_set("utf-8")
MainWindow::MainWindow(QWidget *parent)
    : QMainWindow(parent)
{
    myplan = new Paplan(this);//分配内存
    vpath.ClearPath();//清空路径
    QVector<QPointF> *tpath;
    tpath=vpath.getPath();//将vpath值代入tpath
    veh=new Vehicle;//
    veh->setpath(tpath);//这里将tpath导入到 vehicle中的path
    qDebug()<<(*vpath.getPath()).size();
    qDebug()<<"tpath2";
    lidar.SetInitPos(100,100,0);
    PolarChartInit();
    //添加垂直布局
    QVBoxLayout *layout=new QVBoxLayout;
    btn1=new QPushButton;
    btn2=new QPushButton;
    btn3=new QPushButton;
    btn4=new QPushButton;
    btn5 = new QPushButton;
    btn6 = new QPushButton;
    btn7 = new QPushButton;

    //定时器分配内存，开启定时器
    timer=new QTimer;
    timer->setInterval(time_step);
    timer->start();

    //信号和槽函数
    connect(timer,SIGNAL(timeout()),this,SLOT(ontime()));
    connect(btn1,SIGNAL(clicked()),this,SLOT(onclick1()));
    connect(btn2,SIGNAL(clicked()),this,SLOT(onclick2()));
    connect(btn3,SIGNAL(clicked()),this,SLOT(onclick3()));
    connect(btn4,SIGNAL(clicked()),this,SLOT(onclick4()));
    connect(btn5,SIGNAL(clicked()),this,SLOT(onclick5()));
    connect(btn6,SIGNAL(clicked()),this,SLOT(onclick6()));
    connect(btn7,SIGNAL(clicked()),this,SLOT(onclick7()));
    //按钮命名
    btn1->setText("Vehicle Reset");
    btn2->setText("Vehicle Start");
    btn3->setText("Set Lidar Detecting Path");
    btn4->setText("Set Planned Path");
    btn5->setText("Show Map");
    btn6->setText("Stop Timer");
    btn7->setText("Show Error");
    //标签初始化
    lbl=new QLabel;
    lbl->setText("ready");
    //滑针初始化
    slider=new QSlider;
    slider->setMaximum(255);
    slider->setMinimum(0);
    slider->setValue(100);
    slider->setOrientation(Qt::Horizontal);
    //布局管理器中添加控件
    layout->addWidget(lbl);
    layout->addWidget(slider);
    layout->addWidget(btn1);
    layout->addWidget(btn2);
    layout->addWidget(btn3);
    layout->addWidget(btn4);
    layout->addWidget(btn5);
    layout->addWidget(btn6);
    layout->addWidget(btn7);

    layout->addWidget(chartViewWD);
    //新建控件，并进行参数设定，将布局放入控件中
    QWidget *wg=new QWidget;
    wg->setLayout(layout);
    setCentralWidget(wg);
    resize(600,800);
    move(80,0);

    statusBar();
    lbl1=new QLabel;
    statusBar()->addWidget(lbl1);
    lbl1->setText("ready");

    //雷达探测点容器初始化
    sensorp.resize(720);
    for(int i=0;i<720;i++)
    {
        sensorp[i].setX(double(i)/2);
        sensorp[i].setY(1000);
    }
    lidar.updateSensor(614,419,30,&sensorp);
    PolarChartUpdate(sensorp);


}

MainWindow::~MainWindow()
{

}

//雷达图初始化

void MainWindow::PolarChartInit()
{
    //
       //
       seriesWDP = new QScatterSeries();
       seriesWDP->setName("lidar");
       seriesWDP->setMarkerSize(1);
       seriesWDP->setMarkerShape(QScatterSeries::MarkerShapeCircle);
       seriesWDP->setColor(Qt::red);
       seriesWDP->setBorderColor(Qt::red);
       //
       chartWD = new QPolarChart();
       chartWD->addSeries(seriesWDP);

       chartWD->setTitle("Polor Chart");
       //
       angleAxis = new QValueAxis();
       angleAxis->setTickCount(9);
    //
       angleAxis->setLabelFormat("%d");
       angleAxis->setShadesVisible(true);
       angleAxis->setShadesBrush(QBrush(QColor(249, 249, 255)));
       chartWD->addAxis(angleAxis, QPolarChart::PolarOrientationAngular);
       radialAxis = new QValueAxis();
       radialAxis->setTickCount(4);
       radialAxis->setLabelFormat("%d");
       chartWD->addAxis(radialAxis, QPolarChart::PolarOrientationRadial);
       seriesWDP->attachAxis(radialAxis);
       seriesWDP->attachAxis(angleAxis);
       seriesWDP->setUseOpenGL(true);//openGl 加速
       //
       chartViewWD = new QChartView();
       chartViewWD->setChart(chartWD);
      //

       radialAxis->setRange(0, 750);
       angleAxis->setRange(0, 360);
       //车辆初始化位置
       veh->realxyhInit(1,9,270);

}

void MainWindow::PolarChartUpdate(QVector<QPointF> sv)
{
    //雷达图刷新
    seriesWDP->replace(sv);
}
//定时器中断函数
void MainWindow::ontime()
{
    static QTime time;
    time=QTime::currentTime();

    double interval_time = double(time_step) / 1000;//实际中断时间单位s
    veh->update1(vang,vspd,interval_time);//更新车辆位置

    lidar.updateSensor(veh->x(),veh->y(),veh->headang(),&sensorp);//雷达测得点的更新
    PolarChartUpdate(sensorp);//更新雷达图
    vmap.update(veh->sensorx(),veh->sensory(),veh->sensorheading(),sensorp);//更新检测到地图


    if(vctrl.getAimPoint(*vpath.getPath(),QPointF(veh->realx,veh->realy),veh->realheading,&followErr))//逼近角根据车辆信息进行更新
    {
        vctrl.PIDPositional(&vang,followErr,-2,ki,kd);//P控制
    }
    else
    {//预瞄点不在当前路径了
        if(vehstart)
        {
            vehstart=false;
            vspd=0;
        }

    }

    int a=QTime::currentTime().msecsTo(time);
    lbl->setText("Time:" + QString::number(-a)+"ms");
}

void MainWindow::onclick1()
{
    veh->realxyhInit(1,9,270);//初始化位置
    vehstart=false;//小车停下
    vmap.clear();//清除生成地图的信息
    vctrl.PIDPositional(&vang,followErr,kp,ki,kd,true,0);//P控制初始化
    vspd=0;

}


void MainWindow::onclick2()
{
    vehstart=true;//小车启动
    vspd=300;//速度
//    vspd=500;
}

void MainWindow::onclick3()
{
    vpath.ClearPath();
    vpath.Append(QPointF(8.0,1.0));
    vpath.Append(QPointF(6.0,1.0));
    vpath.Append(QPointF(4.0,1.0));
    vpath.Append(QPointF(4.0,2.9));
    vpath.Append(QPointF(2.0,2.9));
    vpath.Append(QPointF(0.5,2.9));
    vpath.Append(QPointF(0.5,3.5));
    vpath.Append(QPointF(2.7,3.4));
    vpath.Append(QPointF(4.8,3.3));
    vpath.Append(QPointF(4.8,5.5));
    vpath.Append(QPointF(4.8,7.8));
    vpath.Append(QPointF(4.3,8.0));
    vpath.Append(QPointF(4.2,5.0));
    vpath.Append(QPointF(2.5,5.5));
    vpath.Append(QPointF(1.7,6.8));
    vpath.Append(QPointF(1.0,9.0));
    veh->realxyhInit(8.0,1.0,90);
    vehstart=false;
    vmap.clear();
    vctrl.PIDPositional(&vang,followErr,kp,ki,kd,true,0);
    vspd=0;
}

void MainWindow::onclick4()
{
    vpath.ClearPath();

    for ( int i = 1; i < myplan->x.size()-1; i+=45)
    {
        double x1 = myplan->x[i]/100.0;
        double y1 = myplan->y[i]/100.0;
        vpath.Append(QPointF(x1,y1));
        qDebug() << "Path Point:" << x1 << ',' << y1;

    }
    veh->realxyhInit(1.0,9.0,270);
    vehstart=false;
    vmap.clear();
    vctrl.PIDPositional(&vang,followErr,kp,ki,kd,true,0);
    vspd=0;
}

void MainWindow::onclick5()
{
    Mat smap;
    smap = vmap.showmap.clone();
    imshow("Map Detail",smap);
    imwrite("Map.jpg",smap);
}
void MainWindow::onclick6()
{
    clicknum++;
    if(clicknum%2 != 0)
    {
        timer->stop();
        btn6->setText("Start Timer");
    }
    else
    {
        timer->start();
        btn6->setText("Stop Timer");
    }

}
void MainWindow::onclick7()
{
    QPointF a(3.04,6.36727);
    QPointF b(3.04,6.23728);
    QPointF c(2.96515,6.36727);
    double d = 0.0;
    qDebug() << d << HeadingAng(a,c) << HeadingAng(a,b);

}

